
#include "EggNinePCH.h"
#include "mymath.h"

mat4 RotationMatrixFromQ(const vec4& q)
{
#if 0
    __m128 k = (__m128)q;
    __m128 a = _mm_shuffle_ps(k, k, _MM_SHUFFLE(3,3,3,3));
    __m128 b = _mm_shuffle_ps(k, k, _MM_SHUFFLE(0,0,0,0));
    __m128 c = _mm_shuffle_ps(k, k, _MM_SHUFFLE(1,1,1,1));
    __m128 d = _mm_shuffle_ps(k, k, _MM_SHUFFLE(2,2,2,2));

    __m128 v1 = _mm_mul_ps(_mm_mul_ps(b, _mm_setr_ps(1.0f, 2.0f, 2.0f, 0.0f)), k);
    v1 = _mm_add_ps(v1, _mm_mul_ps(_mm_mul_ps(a, _mm_setr_ps(1.0f, 2.0f, -2.0f, 0.0f)),
        _mm_shuffle_ps(k, k, _MM_SHUFFLE(3,1,2,3))));

    __m128 v2 = _mm_mul_ps(_mm_mul_ps(c, _mm_setr_ps(2.0f, 1.0f, 2.0f, 0.0f)), k);
    v2 = _mm_add_ps(v2, _mm_mul_ps(_mm_mul_ps(a, _mm_setr_ps(-2.0f, 1.0f, 2.0f, 0.0f)),
        _mm_shuffle_ps(k, k, _MM_SHUFFLE(3,0,3,2))));

    __m128 v3 = _mm_mul_ps(_mm_mul_ps(d, _mm_setr_ps(2.0f, 2.0f, 1.0f, 0.0f)), k);
    v3 = _mm_add_ps(v3, _mm_mul_ps(_mm_mul_ps(a, _mm_setr_ps(2.0f, -2.0f, 1.0f, 0.0f)),
        _mm_shuffle_ps(k, k, _MM_SHUFFLE(3,3,0,1))));

    __m128 sqK = _mm_mul_ps(k, k);
    __m128 sqK1 = _mm_shuffle_ps(sqK, sqK, _MM_SHUFFLE(2,1,2,0));
    __m128 diag = _mm_hadd_ps(sqK1, sqK);
    diag = _mm_and_ps(diag, _mm_castsi128_ps(_mm_setr_epi32(-1, -1, -1, 0)));

    __m128 tmp = _mm_shuffle_ps(diag, diag, _MM_SHUFFLE(3,3,3,1));  // c*c + d*d
    v1 = _mm_sub_ps(v1, tmp);

    tmp = _mm_shuffle_ps(diag, diag, _MM_SHUFFLE(3,3,0,3));  // b*b + d*d
    v2 = _mm_sub_ps(v2, tmp);

    tmp = _mm_shuffle_ps(diag, diag, _MM_SHUFFLE(3,2,3,3));  // b*b + c*c
    v3 = _mm_sub_ps(v3, tmp);

#else
    // The idea is to sum-factor the matrix into something similar to: diag(r) + (r+r^*-2diag(r))/2 + (r-r^*)/2
    // and manage each individually
    // There's a little twist with the representation of the skew-symmetric matrix
    __m128 k = (__m128)q;
    __m128 masks = _mm_castsi128_ps(_mm_setr_epi32(-1,-1,-1,0));
    __m128 consts = _mm_setr_ps((float)M_SQRT2, (float)(-M_SQRT2), 1.f, 0.f);
    k=_mm_mul_ps(k, _mm_shuffle_ps(consts, consts, _MM_SHUFFLE(0,0,0,0)));

    // skew-symmetric components
    __m128 ss=_mm_mul_ps(k, _mm_shuffle_ps(k, k, _MM_SHUFFLE(3,3,3,3)));
    ss=_mm_and_ps(ss, masks);
    
    // symmetric components
    __m128 s=_mm_mul_ps(_mm_shuffle_ps(k, k, _MM_SHUFFLE(3,0,0,1)), _mm_shuffle_ps(k, k, _MM_SHUFFLE(3,1,2,2)));
    s=_mm_and_ps(s, masks);

    // diagonal
    __m128 sqK=_mm_mul_ps(k, k);
    __m128 diag=_mm_sub_ps(_mm_shuffle_ps(consts, consts, _MM_SHUFFLE(3,2,2,2)), _mm_shuffle_ps(sqK, sqK, _MM_SHUFFLE(3,0,0,1)));
    diag=_mm_sub_ps(diag, _mm_shuffle_ps(sqK, sqK, _MM_SHUFFLE(3,1,2,2)));
    diag=_mm_and_ps(diag, masks);

    // combine components
    __m128 plusS=_mm_add_ps(s, ss);
    __m128 minusS=_mm_sub_ps(s, ss);

    // here comes some shuffle magic...
    __m128 v1=_mm_shuffle_ps(diag, plusS, _MM_SHUFFLE(2,2,0,0));
    v1=_mm_shuffle_ps(v1, minusS, _MM_SHUFFLE(3,1,2,0));
    __m128 v2=_mm_shuffle_ps(diag, minusS, _MM_SHUFFLE(2,2,1,1));
    v2=_mm_shuffle_ps(v2, plusS, _MM_SHUFFLE(3,0,1,2));
    __m128 v3=_mm_shuffle_ps(minusS, plusS, _MM_SHUFFLE(1,1,0,0));
    v3=_mm_shuffle_ps(v3, diag, _MM_SHUFFLE(3,2,0,2));
#endif
    mat4 m;
    m(0)=v1;
    m(1)=v2;
    m(2)=v3;

    return m;
}

mat4 RotateAroundAxis(float x, float y, float z, float angle)
{
    // use Rodriguez's rotation formula
    mat4 kx;
    __m128 k;
    __m128i tmp, signs;

    k = _mm_setr_ps(x, y, z, 0);
    signs = _mm_cvtsi32_si128(1<<31);
    tmp = _mm_castps_si128(k);

    kx(1) = _mm_castsi128_ps(_mm_xor_si128(_mm_shuffle_epi32(tmp, _MM_SHUFFLE(3, 0, 3, 2)), signs));

    signs = _mm_slli_si128(signs, 4);
    kx(2) = _mm_castsi128_ps(_mm_xor_si128(_mm_shuffle_epi32(tmp, _MM_SHUFFLE(3, 3, 0, 1)), signs));

    signs = _mm_slli_si128(signs, 4);
    kx(0) = _mm_castsi128_ps(_mm_xor_si128(_mm_shuffle_epi32(tmp, _MM_SHUFFLE(3, 1, 2, 3)), signs));

    kx(3) = _mm_setzero_ps();

    kx*=sin(angle);

    mat4 kk;
    kk(0) = _mm_mul_ps(k, _mm_shuffle_ps(k, k, _MM_SHUFFLE(3,0,0,0)));
    kk(1) = _mm_mul_ps(k, _mm_shuffle_ps(k, k, _MM_SHUFFLE(3,1,1,1)));
    kk(2) = _mm_mul_ps(k, _mm_shuffle_ps(k, k, _MM_SHUFFLE(3,2,2,2)));
    kk-=mat4();
    kk*=1-cos(angle);
    kk+=mat4();

    kk+=kx;
    kk(3)=_mm_setr_ps(0,0,0,1);

    return kk;
}
